/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* E.P. Scene Graph
* Developed by:
*       - Eduardo Ribeiro (eribeiro @ tecgraf . puc-rio . br)
*       - Peter Dam (peter @ tecgraf . puc-rio . br)
* File: Mat4.h
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef _EPSG_MAT4_
#define _EPSG_MAT4_

#include <math.h>

namespace EPSG
{
	template <typename T>
	class Mat4
	{
	public:
		/* [CONSTRUCTOR] */	Mat4();

		T&					operator[]( int index );
		void				operator=( Mat4<T>& other );
		void				operator*=( Mat4<T>& other );
		
		T&					get( int i, int j );
		T*					getAsArray();
		void				setIdentity();
		Mat4<T>				invert();

		static Mat4<T>		zero();

		T					values[16];

	private:
		T					invalid;
	};
}

/* TEMPLATE METHODS */
template <typename T>
EPSG::Mat4<T>::Mat4()
{
	setIdentity();
}

template <typename T>
T& EPSG::Mat4<T>::operator[]( int index )
{
	if( index < 0 || index > 16 )
		return invalid;

	return values[index];
}

template <typename T>
void EPSG::Mat4<T>::operator=( EPSG::Mat4<T>& other )
{
	for( int i = 0 ; i < 16; i++ )
		values[i] = other[i];
}

template <typename T>
void EPSG::Mat4<T>::operator*=( EPSG::Mat4<T>& other )
{
	Mat4<T> ret;
	for( int i = 0; i < 16; i++ )
	{
		ret[i] = 
			values[0 + ((int)(i / 4) * 4)] * other[i % 4 +  0] +
			values[1 + ((int)(i / 4) * 4)] * other[i % 4 +  4] +
			values[2 + ((int)(i / 4) * 4)] * other[i % 4 +  8] +
			values[3 + ((int)(i / 4) * 4)] * other[i % 4 + 12];
	}
	*this = ret;
}

template <typename T>
T& EPSG::Mat4<T>::get( int i, int j )
{
	if( i*4 + j < 0 || i*4 + j > 16 )
		return invalid;

	return values[i*4+j];
}

template <typename T>
T* EPSG::Mat4<T>::getAsArray()
{
	return values;
}

template <typename T>
void EPSG::Mat4<T>::setIdentity()
{
	for( int i = 0; i < 16; i++ )
	{
		if( i % 4 == i / 4 )
			values[i] = (T)1;
		else
			values[i] = (T)0;
	}
}

template <typename T>
EPSG::Mat4<T> EPSG::Mat4<T>::invert()
{
	T fA0 = values[ 0]*values[ 5] - values[ 1]*values[ 4];
	T fA1 = values[ 0]*values[ 6] - values[ 2]*values[ 4];
	T fA2 = values[ 0]*values[ 7] - values[ 3]*values[ 4];
	T fA3 = values[ 1]*values[ 6] - values[ 2]*values[ 5];
	T fA4 = values[ 1]*values[ 7] - values[ 3]*values[ 5];
	T fA5 = values[ 2]*values[ 7] - values[ 3]*values[ 6];
	T fB0 = values[ 8]*values[13] - values[ 9]*values[12];
	T fB1 = values[ 8]*values[14] - values[10]*values[12];
	T fB2 = values[ 8]*values[15] - values[11]*values[12];
	T fB3 = values[ 9]*values[14] - values[10]*values[13];
	T fB4 = values[ 9]*values[15] - values[11]*values[13];
	T fB5 = values[10]*values[15] - values[11]*values[14];

	T fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
	if( fabs( (float)fDet ) <= 1e-06f )
		return Mat4<T>::zero();

	Mat4<T> ret;
	ret.values[ 0] =
		+ values[ 5]*fB5 - values[ 6]*fB4 + values[ 7]*fB3;
	ret.values[ 4] =
		- values[ 4]*fB5 + values[ 6]*fB2 - values[ 7]*fB1;
	ret.values[ 8] =
		+ values[ 4]*fB4 - values[ 5]*fB2 + values[ 7]*fB0;
	ret.values[12] =
		- values[ 4]*fB3 + values[ 5]*fB1 - values[ 6]*fB0;
	ret.values[ 1] =
		- values[ 1]*fB5 + values[ 2]*fB4 - values[ 3]*fB3;
	ret.values[ 5] =
		+ values[ 0]*fB5 - values[ 2]*fB2 + values[ 3]*fB1;
	ret.values[ 9] =
		- values[ 0]*fB4 + values[ 1]*fB2 - values[ 3]*fB0;
	ret.values[13] =
		+ values[ 0]*fB3 - values[ 1]*fB1 + values[ 2]*fB0;
	ret.values[ 2] =
		+ values[13]*fA5 - values[14]*fA4 + values[15]*fA3;
	ret.values[ 6] =
		- values[12]*fA5 + values[14]*fA2 - values[15]*fA1;
	ret.values[10] =
		+ values[12]*fA4 - values[13]*fA2 + values[15]*fA0;
	ret.values[14] =
		- values[12]*fA3 + values[13]*fA1 - values[14]*fA0;
	ret.values[ 3] =
		- values[ 9]*fA5 + values[10]*fA4 - values[11]*fA3;
	ret.values[ 7] =
		+ values[ 8]*fA5 - values[10]*fA2 + values[11]*fA1;
	ret.values[11] =
		- values[ 8]*fA4 + values[ 9]*fA2 - values[11]*fA0;
	ret.values[15] =
		+ values[ 8]*fA3 - values[ 9]*fA1 + values[10]*fA0;

	T fInvDet = ((T)1.0)/fDet;
	ret.values[ 0] *= fInvDet;
	ret.values[ 1] *= fInvDet;
	ret.values[ 2] *= fInvDet;
	ret.values[ 3] *= fInvDet;
	ret.values[ 4] *= fInvDet;
	ret.values[ 5] *= fInvDet;
	ret.values[ 6] *= fInvDet;
	ret.values[ 7] *= fInvDet;
	ret.values[ 8] *= fInvDet;
	ret.values[ 9] *= fInvDet;
	ret.values[10] *= fInvDet;
	ret.values[11] *= fInvDet;
	ret.values[12] *= fInvDet;
	ret.values[13] *= fInvDet;
	ret.values[14] *= fInvDet;
	ret.values[15] *= fInvDet;

	return ret;
}

template <typename T>
EPSG::Mat4<T> EPSG::Mat4<T>::zero()
{
	Mat4<T> ret;
	
	for( int i = 0; i < 16; i++ )
		ret[i] = 0;

	return ret;
}

#endif //_EPSG_MAT4_